/*
 * @Author: Mcfly
 * @Date: 2021-07-31 01:39:17
 * @LastEditTime: 2021-11-02 21:07:24
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\BSPdriver\OpticalFlowYTVG.c
 * 本人github:https://github.com/McflyWZX
 */
#include "OpticalFlowYTVG.h"
#include "stdlib.h"
#include "Usart.h"
#include "uart.h"
#include "Time.h"

FlowYTVG *FlowYTVG_Init(void)
{
    FlowYTVG *flow = (FlowYTVG *)malloc(sizeof(FlowYTVG));

    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
    GPIOPinTypeGPIOOutput(GPIO_PORTD_BASE, GPIO_PIN_0);
    FlowYTVG_EnableRotate(0);
  
    flow->deltaT = 0;
    flow->recvState = 0;
    flow->optX = 0;
    flow->optY = 0;
    flow->SQUAL = 0;
    flow->dataLoop = 0;
	return flow;
}

uint32_t lastOKdataTime = 0;
uint8_t recvBuf[6];
uint8_t recvIndex;
uint8_t checkSumTemp;
void FlowYTVG_Recv(FlowYTVG *flow, uint8_t recv)
{
    switch(flow->recvState)
    {
        case 0:     //接收完一帧数据后，检查包头
            if(recv == 0xFE)
                flow->recvState = 1;
            else 
                return;
            break;
        case 1:     //检查包头第二字节
            if(recv == 0x04)
            {
                flow->recvState = 2;
                recvIndex = 0;
            } else {
                flow->recvState = 0;
                return;
            }
            break;
        case 2:     //正常接收buf
            if(recvIndex < 6)
            {
                recvBuf[recvIndex++] = recv;
            } else {
                if(recv == 0xAA || recv == 0xBB)
                {
                    checkSumTemp = recvBuf[0] + recvBuf[1] + recvBuf[2] + recvBuf[3];
                    if(checkSumTemp == recvBuf[4]) {
                        int tempX = (int16_t)(recvBuf[1] << 8)|recvBuf[0];
                        int tempY = (int16_t)(recvBuf[3] << 8)|recvBuf[2];
                        flow->optX = 0.48f * tempX + 0.52f * flow->optX;
                        flow->optY = 0.48f * tempY + 0.52f * flow->optY;
                        flow->SQUAL = recvBuf[5];
                        flow->dataLoop = 1;
                        flow->deltaT = micros() - lastOKdataTime;
                        lastOKdataTime = micros();
                    }
                }
                flow->recvState = 0;
                return;
            }
            break;
    }
}

uint8_t FlowYTVG_CheckData(FlowYTVG *flow)
{
	return 0;
}

void FlowYTVG_EnableRotate(uint8_t isEnable)
{
    GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_0, !isEnable);
}
